#ifndef PID_H
#define PID_H
#include <Eigen/Eigen>


inline void Pd_limited(Eigen::Vector3d &force,const double &kp,const double &kd, const double force_limit,
                                       const Eigen::Vector3d &target_pos, const Eigen::Vector3d &cur_pos,
                                       const Eigen::Vector3d &target_vel=Eigen::Vector3d::Zero(), const Eigen::Vector3d &cur_vel=Eigen::Vector3d::Zero())
{
    force=kp*(target_pos-cur_pos)+kd*(target_vel-cur_vel);
    double scale=force_limit/fmax(fabs(force.minCoeff()),fabs(force.maxCoeff()));
    force=force*fmin(scale,1.0);
}
#endif // PID_H
